exception NothingToDo

let rec add color = function
  | [] -> (color, 1)::[]
  | (c, nb)::l when c = color -> (c, nb+1)::l
  | h::l -> h::(add color l)

let rec search color = function
  | [] -> -1
  | (c, rp)::_ when c = color -> rp
  | _::l -> search color l

let rec convert cA i = function
    [] -> ()
  | h::t -> !cA.(i) <- h; convert cA (i+1) t

let histo mtrx_base w h min =
  let colorList = ref [] in
    for x = 0 to (w - 1) do
      for y = 0 to (h - 1) do
	colorList := (add mtrx_base.(x).(y) !colorList);
      done;
    done;
    let colorArray =
      ref (Array.make (List.length !colorList) ((0.,0.,0.), 0)) in
      convert colorArray 0 !colorList;
      colorList := [];
      for i = 0 to ((Array.length !colorArray) - 1) do
	let ((r, g, b), nb) = !colorArray.(i) in
	  if nb < min then
	    begin
	      let closer = ref 25000. in
	      let closerIndex = ref 0 in
		for j = 0 to ((Array.length !colorArray) - 1) do
		  let ((r2, g2, b2), nb) = !colorArray.(j) in
		    if nb > min then
		      begin
			let nr = (r -. r2) in
			let ng = (g -. g2) in
			let nb = (b -. b2) in
			let n = sqrt (nr*.nr +. ng*.ng +. nb*.nb) in
			  if n < !closer then
			    begin
			      closer := n;
			      closerIndex := j;
			    end;
		      end;
		done;
		colorList := ((r, g, b), !closerIndex)::!colorList;
	    end;
      done;
      for x = 0 to (w - 1) do
	for y = 0 to (h - 1) do
	  let index = search mtrx_base.(x).(y) !colorList in
	    if index = -1 then
	      ()
	    else
	      begin
		let (c, nb) = !colorArray.(index) in
		  mtrx_base.(x).(y) <- c;
	      end
	done;
      done


let creat_blur_mtrx size sigma =
  let g = Array.init (size*2+1) (fun _ -> Array.make (size*2+1) 0.) in
  let div = ref 0. in
    for x = -size to size do
      for y = -size to size do
	if x = 0 && y = 0 then
	  begin
	    let value = 0. in
	      div := !div +. value;
              g.(x+size).(y+size) <- value;
	  end
	else
	  begin
	    let value = exp (-.(((float_of_int x) ** 2.)
				+. ((float_of_int y) ** 2.)))
	      /. 2.*.(sigma**2.) in
	      div := !div +. value;
	      g.(x+size).(y+size) <- value;
	  end;
      done;
    done;
    for x = -size to size do
      for y = -size to size do
	g.(x+size).(y+size) <- g.(x+size).(y+size) /. !div;
      done;
    done;
    g


let times n (x, y, z) = (n*.x, n*.y, n*.z)
let add (x, y, z) (a, b, c) = (a+.x, b+.y, c+.z)
let toint (x, y, z) = (int_of_float x, int_of_float y, int_of_float z)
let tofloat (x, y, z) = (float_of_int x, float_of_int y, float_of_int z)


let apply g x y size w h m img_blur =
  let newcolor = ref (0., 0., 0.) in

    if x >= size && y >= size && x < (w-size) && y < (h-size) then
      begin
	for i = -size to size do
	  for j = -size to size do
	    newcolor := add !newcolor
	      (times g.(size+i).(size+j) !m.(x + i).(y + j));
	  done;
	done;
	Sdlvideo.put_pixel_color !img_blur x y (toint !newcolor);
      end

    else
      begin

	if x < size then
	  begin
	    for i = -x to size do

	      if y < size then
		begin
		  for j = -y to size do
		    newcolor := add !newcolor
		      (times g.(i+size).(j+size) !m.(x + i).(y + j));
		  done;
		end

	      else if y >= size && y < (h-size) then
		begin
		  for j = -size to size do
		    newcolor := add !newcolor
                      (times g.(i+size).(j+size) !m.(x + i).(y + j));
                  done;
		end

	      else
		begin
		  for j = -size to (h-y-1) do
		    newcolor := add !newcolor
                      (times g.(i+size).(j+size) !m.(x + i).(y + j));
                  done;
		end

	    done;
	    Sdlvideo.put_pixel_color !img_blur x y (toint !newcolor)
	  end

	else if x > (w-size-1) then
	  begin
	    for i = -size to (w-x-1) do

	      if y < size then
		begin
		  for j = -y to size do
		    newcolor := add !newcolor
		      (times g.(i+size).(j+size) !m.(x + i).(y + j));
		  done;
		end

	      else if y >= size && y < (h-size) then
		begin
		  for j = -size to size do
		    newcolor := add !newcolor
                      (times g.(i+size).(j+size) !m.(x + i).(y + j));
                  done;
		end

	      else
		begin
		  for j = -size to (h-y-1) do
		    newcolor := add !newcolor
                      (times g.(i+size).(j+size) !m.(x + i).(y + j));
                  done;
		end;

	    done;
	    Sdlvideo.put_pixel_color !img_blur x y (toint !newcolor);
	  end

	else if y < size then
	  begin
	    for i = -size to size do
	      for j = -y to size do
		newcolor := add !newcolor
		  (times g.(i+size).(j+size) !m.(x + i).(y + j));
              done;
	    done;
	    Sdlvideo.put_pixel_color !img_blur x y (toint !newcolor);
	  end

	else
	  begin
	    for i = -size to size do
	      for j = -size to (h-y-1) do
		newcolor := add !newcolor
		  (times g.(i+size).(j+size) !m.(x + i).(y + j));
	      done;
	    done;
	    Sdlvideo.put_pixel_color !img_blur x y (toint !newcolor)
	  end
      end

let blur mtrx_base w h size sigma img_blur img_path =

  let img = ref img_blur in
  let m = ref mtrx_base in
  let g = creat_blur_mtrx size sigma in
    for x = 0 to (w - 1) do
      for y = 0 to (h - 1) do
	apply g x y size w h m img;
      done;
    done

let get_dims img = ((Sdlvideo.surface_info img).Sdlvideo.w,
                    (Sdlvideo.surface_info img).Sdlvideo.h)


let execute img_base img_nc_path tostdout =
  let (w, h) = get_dims img_base in
  let mtrx_base =  Array.init w (fun _ -> Array.make h (0., 0., 0.)) in
    for x = 0 to (w-1) do
      for y = 0 to (h-1) do
	mtrx_base.(x).(y) <- tofloat
	  (Sdlvideo.get_pixel_color img_base x y);
      done;
    done;
    let img_blur = Sdlvideo.create_RGB_surface_format img_base [] w h in
      blur mtrx_base w h 2 1. img_blur "testblur.bmp";
      let mtrx_blur =  Array.init w (fun _ -> Array.make h (0., 0., 0.)) in
	for x = 0 to (w-1) do
	  for y = 0 to (h-1) do
	    mtrx_blur.(x).(y) <- tofloat
	      (Sdlvideo.get_pixel_color img_blur x y);
	  done;
	done;
	let img_nc = Sdlvideo.create_RGB_surface_format img_base [] w h in
	  histo mtrx_blur w h 1000;
	  for x = 0 to (w-1) do
	    for y = 0 to (h-1) do
	      Sdlvideo.put_pixel_color img_nc x y (toint mtrx_blur.(x).(y))
	    done;
	  done;
   if tostdout then
     begin
       let out = Filename.temp_file "none" ".bmp"
       in
         Sdlvideo.save_BMP img_nc out;
       let buf = open_in_bin out
       in
         try
           while true do
             print_char (input_char buf)
           done
         with _-> close_in buf; ()
      end
